#include <pic18f4431.h>

#define VERSION "0203F0"
 
/* MicroController Frequency */
#define FOSC 40000000
#define TOSC 25

/* Leds */
#define LED_YELLOW PORTCbits.RC2
#define LED_GREEN PORTCbits.RC1
#define SETLEDOUTPUTPORTS TRISCbits.TRISC1=TRISCbits.TRISC2=0

/* Gears/Worms/Steppers */
#define RA_NB_TEETH 144
#define DE_NB_TEETH 144
#define RA_GEAR_RATIO 4
#define DE_GEAR_RATIO 6
#define RA_NB_STEPS 400
#define DE_NB_STEPS 400
#define MICROSTEPS 8
/* #define RA_STEPS_360 (RA_NB_TEETH * RA_GEAR_RATIO * RA_NB_STEPS * MICROSTEPS) */
/* #define DE_STEPS_360 (DE_NB_TEETH * DE_GEAR_RATIO * DE_NB_STEPS * MICROSTEPS) */
#define RA_STEPS_360  0x1C2000
#define DE_STEPS_360  0x2A3000
/* #define RA_STEPS_WORM (RA_GEAR_RATIO * RA_NB_STEPS * MICROSTEPS) */
/* #define DE_STEPS_360 (DE_GEAR_RATIO * DE_NB_STEPS * MICROSTEPS) */
#define RA_STEPS_WORM  0x3200
#define DE_STEPS_WORM  0x4B00

/** Motors **/
#define PWMPERIOD 0x007F
#define RAPHIA PORTDbits.RD5
#define RAPHIB PORTDbits.RD6
#define RAENABLE PORTBbits.RB0
// ADPWMA = PWM1
#define RAPWMA PORTBbits.RB1 
// ADPWMB = PWM5
#define RAPWMB PORTBbits.RB4
#define SETRAOUTPUTPORTS TRISDbits.TRISD5=TRISDbits.TRISD6=TRISBbits.TRISB0=0
#define SETRAPWMA(duty) PDC0L = duty & 0xFF; PDC0H = (duty >> 8) & 0x3F
#define SETRAPWMB(duty) PDC2L = duty & 0xFF; PDC2H = (duty >> 8) & 0x3F

#define DEPHIA PORTCbits.RC4
#define DEPHIB PORTCbits.RC5
#define DEENABLE PORTDbits.RD4
// DEPWMA = PWM3
#define DEPWMA PORTBbits.RB3 
// DEPWMB = PWM7
#define DEPWMB PORTDbits.RD7
#define SETDEOUTPUTPORTS TRISCbits.TRISC4=TRISCbits.TRISC5=TRISDbits.TRISD4=0
#define SETDEPWMA(duty) PDC1L = duty & 0xFF; PDC1H = (duty >> 8) & 0x3F
#define SETDEPWMB(duty) PDC3L = duty & 0xFF; PDC3H = (duty >> 8) & 0x3F

/* Microstepping */
/* 8 microsteps */
#define MICROSTEP_MASK 0x07
// PWM_MASK = 0x1 << LN(MICROSTEPS)
#define PWM_MASK 0x08 
// WINDINGB_MASK = 0x2 << LN(MICROSTEPS)
#define WINDINGB_MASK 0x10
// WINDINGA_MASK = 0x3 << LN(MICROSTEPS)
#define WINDINGA_MASK 0x18

#define SETRAPHIA(microstep) RAPHIA = ((((microstep & WINDINGA_MASK) == 0) || ((microstep & WINDINGA_MASK) == WINDINGA_MASK)) ? 1 : 0)
#define SETRAPHIB(microstep) RAPHIB = ((microstep & WINDINGB_MASK) ? 1 : 0)

#define SETDEPHIA(microstep) DEPHIA = (((microstep & WINDINGA_MASK) == 0) || ((microstep & WINDINGA_MASK) == WINDINGA_MASK)) ? 1 : 0
#define SETDEPHIB(microstep) DEPHIB = (microstep & WINDINGB_MASK) ? 1 : 0

/* Motor status (Skywatcher protocol) */
#define INITIALIZED 0x0100
#define RUNNING 0x0001
#define SLEWMODE 0X0010
#define BACKWARD 0x0020
#define HIGHSPEED 0x0040
#define SETMOTORPROPERTY(motorstatus, property) motorstatus |= property
#define UNSETMOTORPROPERTY(motorstatus, property) motorstatus &= ~property
#define GETMOTORPROPERTY(motorstatus, property) (motorstatus & property)

/* Timer period */
#define MICROSECONDS 1000000
#define MUL_RA (MICROSECONDS / RA_STEPS_WORM)
#define REM_RA (MICROSECONDS % RA_STEPS_WORM)
#define MUL_DE (MICROSECONDS / DE_STEPS_WORM)
#define REM_DE (MICROSECONDS % DE_STEPS_WORM)
#define TICKS_MICRO 1000 / (4 * TOSC)
#define MIN_TICKS (PWMPERIOD + 1) * 16


typedef union {
  unsigned int value;
  struct { unsigned char valuel; unsigned char valueh; } fields;
} TimerRegister;

typedef struct {
  unsigned char count;
  unsigned char current;
  TimerRegister timer;
} TimerValue;

#define SETTIMER0(timer) TMR0H=timer.fields.valueh;TMR0L=timer.fields.valuel
#define SETTIMER1(timer) TMR1H=timer.fields.valueh;TMR1L=timer.fields.valuel

/* Motor acceleration/deceleration */
// Speed ramp states
#define STOP  0
#define ACCEL 1
#define DECEL 2
#define RUN   3

typedef struct {
  //! What part of the speed ramp we are in.
  unsigned char run_state : 3;
  //! Peroid of next timer delay. At start this value set the accelration rate.
  unsigned long step_delay;
  unsigned int rest;
  //! What step_pos to start decelaration
  unsigned long decel_start;
  //! Sets deceleration rate.
  unsigned int decel_val;
  //! Minimum time delay (max speed)
  unsigned long min_delay;
  //! Counter used when accelerateing/decelerateing to calculate step_delay.
  unsigned int accel_count;
} speedRampData;

/* Acceleration parameters (from http://atmel.com/dyn/resources/prod_documents/doc8017.pdf) */
/* http://eetimes.com/design/embedded/4006438/Generate-stepper-motor-speed-profiles-in-real-time */
#define TIMER_FREQ (MICROSECONDS * TICKS_MICRO)
#define ALPHA (2*M_PI) / RA_NBSTEPS 
#define AT_100 (ALPHA * TIMER_FREQ * 100)
#define TFREQ (TIMER_FREQ * 0.676) / 100
#define A_SQ (2 * ALPHA * 10000000000)
#define A_x20000 (2 * ALPHA * 10000)
/* c0 = f * sqrt(2 * alpha / accel) */
/* for accel = (M_PI * 2) rad/s2 */
/*#define C0 707106
#define C0_APPROX 478004
#define C0_TIMER_COUNT 8
#define C0_TIMER_VALUEH 0x16
#define C0_TIMER_VALUEL 0x9A
*/
/* for accel = (M_PI * 4) rad/s2 */
#define C0 500000
#define C0_APPROX 338000
#define C0_TIMER_COUNT 8
#define C0_TIMER_VALUEH 0x5A
#define C0_TIMER_VALUEL 0xF6

